#pragma once

#include <vector>

#include "../core/i_rand.h"
#include "../geometry/i_util.h"
#include "../geometry/i_triangulation.h"
#include "../geometry/i_twoview.h"
#include "i_ncc.h"
#include "i_grad.h"

namespace idl
{
  template<typename ImageType, typename DepthType>
  class BinoCorrespondence
  {
    public:
      BinoCorrespondence();
      BinoCorrespondence(int width, int height, int ncc_patch_width, int geo_uncert_rad = 0, int grad_patch_width = 3, int rank_patch_width = 3);
      ~BinoCorrespondence();
      int match(const ImageType *const *im1,
                const DepthType K1[9], const DepthType R1[9], const DepthType t1[3],
                const ImageType *const *im2,
                const DepthType K2[9], const DepthType R2[9], const DepthType t2[3],
                DepthType **depthmap, /*treat im1 as reference view*/
                DepthType z_near,
                DepthType z_far,
                int method = 0,
                float match_cost_tol = 0.5f,
                float match_ratio_threshold = 0.75f, /*threshold for the matching ratio test*/
                int x0 = 0, /*starting pixel's x coord.*/
                int y0 = 0, /*starting pixel's y coord.*/
                int dx = 1, /*sampling freq. along the x dimension.*/
                int dy = 1, /*sampling freq. along the y dimension.*/         
                DepthType empty_val = (DepthType)0.0, /*every pixel that has not been matched will be assigned this value*/
                const unsigned char *const *match_mask = NULL/*binary mask with values 0: non-edge pixel, 1: edge pixel*/);

    protected:
      int _width, _height, _ncc_patch_width, _geo_uncert_rad, _grad_patch_width, _rank_patch_width;
      bool _initialized;
    protected:
      int match_image_ncc(const ImageType *const *im1,
                          const DepthType K1[9], const DepthType R1[9], const DepthType t1[3],
                          const ImageType *const *im2,
                          const DepthType K2[9], const DepthType R2[9], const DepthType t2[3],
                          DepthType **depthmap, /*treat im1 as reference view*/
                          DepthType z_near,
                          DepthType z_far,
                          float min_similarity_score_tol,
                          float match_ratio_threshold, /*threshold for the matching ratio test*/
                          int x0, /*starting pixel's x coord.*/
                          int y0, /*starting pixel's y coord.*/
                          int dx, /*sampling freq. along the x dimension.*/
                          int dy, /*sampling freq. along the y dimension.*/   
                          /*set (dx=1 & dy=1) leads to dense matching, not suggested because the way matching is performed in this function is not efficient for dense matching*/
                          DepthType empty_val /*every pixel that has not been matched will be assigned this value*/
                          );

      int match_image_ncc(const ImageType *const *im1,
                           const DepthType K1[9], const DepthType R1[9], const DepthType t1[3],
                           const ImageType *const *im2,
                           const DepthType K2[9], const DepthType R2[9], const DepthType t2[3],
                           DepthType **depthmap, /*treat im1 as reference view*/
                           DepthType z_near,
                           DepthType z_far,
                           float min_similarity_score_tol,
                           float match_ratio_threshold, /*threshold for the matching ratio test*/
                           int x0, /*starting pixel's x coord.*/
                           int y0, /*starting pixel's y coord.*/
                           int dx, /*sampling freq. along the x dimension.*/
                           int dy, /*sampling freq. along the y dimension.*/
                           /*set (dx=1 & dy=1) leads to dense matching, not suggested because the way matching is performed in this function is not efficient for dense matching*/
                           DepthType empty_val, /*every pixel that has not been matched will be assigned this value*/
                           const unsigned char *const *match_mask /*binary mask with values 0: non-edge pixel, 1: edge pixel*/);

      int match_image_grad(const ImageType *const *im1,
                          const DepthType K1[9], const DepthType R1[9], const DepthType t1[3],
                          const ImageType *const *im2,
                          const DepthType K2[9], const DepthType R2[9], const DepthType t2[3],
                          DepthType **depthmap, /*treat im1 as reference view*/
                          DepthType z_near,
                          DepthType z_far,
                          float min_similarity_score_tol,
                          float match_ratio_threshold, /*threshold for the matching ratio test*/
                          int x0, /*starting pixel's x coord.*/
                          int y0, /*starting pixel's y coord.*/
                          int dx, /*sampling freq. along the x dimension.*/
                          int dy, /*sampling freq. along the y dimension.*/
                          /*set cycle=1 leads to dense matching, in tehory (1/cycle) random pixel go through matching, not suggested because the way matching is performed in this function is not efficient for dense matching*/
                          DepthType empty_val, /*every pixel that has not been matched will be assigned this value*/
                          const unsigned char *const *match_mask /*binary mask with values 0: non-edge pixel, 1: edge pixel*/);


      int match_image_randomized_ncc_grad(const ImageType *const *im1,
                                          const DepthType K1[9], const DepthType R1[9], const DepthType t1[3],
                                          const ImageType *const *im2,
                                          const DepthType K2[9], const DepthType R2[9], const DepthType t2[3],
                                          DepthType **depthmap, /*treat im1 as reference view*/
                                          DepthType z_near,
                                          DepthType z_far,
                                          float min_similarity_score_tol,
                                          float match_ratio_threshold, /*threshold for the matching ratio test*/
                                          int cycle, 
                                          /*set cycle=1 leads to dense matching, in tehory (1/cycle) random pixel go through matching, not suggested because the way matching is performed in this function is not efficient for dense matching*/
                                          DepthType empty_val, /*every pixel that has not been matched will be assigned this value*/
                                          const unsigned char *const *match_mask /*binary mask with values 0: non-edge pixel, 1: edge pixel*/);

      bool match_line_ncc(const CorrespondenceNCC<ImageType> &patch,
                          const ImageType *const *im2,
                          const DepthType line_endpt_coord1[2], /*inhomogeneous 2D image coordinates*/
                          const DepthType line_endpt_coord2[2], /*inhomogeneous 2D image coordinates*/
                          DepthType matchpt[2],
                          float min_similarity_score_tol,
                          float match_ratio_threshold /*threshold for the matching ratio test*/);

      bool match_line_grad(const CorrespondenceGrad<ImageType> &grad_patch,
                           const ImageType *const *im2,
                           const DepthType line_endpt_coord1[2], /*inhomogeneous 2D image coordinates*/
                           const DepthType line_endpt_coord2[2], /*inhomogeneous 2D image coordinates*/
                           DepthType matchpt[2],
                           float min_similarity_score_tol,
                           float match_ratio_threshold /*threshold for the matching ratio test*/);

      bool match_pixel_ncc(const CorrespondenceNCC<ImageType> &patch,
                           const ImageType *const *im2,
                           const DepthType coord[2], /*inhomogeneous 2D image coordinates*/
                           float &ncc_similarity_score);

      bool match_pixel_grad(const CorrespondenceGrad<ImageType> &grad_patch,
                            const ImageType *const *im2,
                            const DepthType coord[2], /*inhomogeneous 2D image coordinates*/
                            float &grad_similarity_score);
      
      inline bool ratiotest(float best, float second_best, float match_ratio_threshold)
      {
        if (best <= 0.0f)
        {
          return false;
        }
        return i_div(best - second_best, best) > match_ratio_threshold;
      }
  };

  template<typename ImageType, typename DepthType>
  BinoCorrespondence<typename ImageType, typename DepthType>::BinoCorrespondence() :_width(0), _height(0), _ncc_patch_width(0), _geo_uncert_rad(0), _grad_patch_width(0), _rank_patch_width(0), _initialized(false){}
  
  template<typename ImageType, typename DepthType>
  BinoCorrespondence<typename ImageType, typename DepthType>::BinoCorrespondence(int width, int height, int ncc_patch_width, int geo_uncert_rad, int grad_patch_width, int rank_patch_width) : _width(width), _height(height), _ncc_patch_width(ncc_patch_width), _geo_uncert_rad(geo_uncert_rad), _grad_patch_width(grad_patch_width), _rank_patch_width(rank_patch_width)
  {
    _initialized = true;
  }

  template<typename ImageType, typename DepthType>
  BinoCorrespondence<typename ImageType, typename DepthType>::~BinoCorrespondence(){}

  template<typename ImageType, typename DepthType>
  int BinoCorrespondence<typename ImageType, typename DepthType>::match(const ImageType *const *im1,
                                                                        const DepthType K1[9], const DepthType R1[9], const DepthType t1[3],
                                                                        const ImageType *const *im2,
                                                                        const DepthType K2[9], const DepthType R2[9], const DepthType t2[3],
                                                                        DepthType **depthmap, /*treat im1 as reference view*/
                                                                        DepthType z_near,
                                                                        DepthType z_far,
                                                                        int method,
                                                                        float match_cost_tol,
                                                                        float match_ratio_threshold, /*threshold for the matching ratio test*/
                                                                        int x0,
                                                                        int y0,
                                                                        int dx,
                                                                        int dy,                                                                      
                                                                        DepthType empty_val, /*every pixel that has not been matched will be assigned this value*/
                                                                        const unsigned char *const *match_mask/*binary mask that indicated witch pixels need to be matched*/)
  {
    int rtv = -1;
    if (!_initialized)
    {
      return -2; /*error: initialization error*/
    }
    if (!im1 || !K1 || !R1 || !t1 || !im2 || !K2 || !R2 || !t2 || !depthmap)
    {
      return -1; /*error: invalid input*/
    }
    switch (method)
    {
      case 0: 
        if (match_mask != NULL)
        {
          rtv = match_image_ncc(im1, K1, R1, t1, im2, K2, R2, t2, depthmap, z_near, z_far, match_cost_tol, match_ratio_threshold, x0, y0, dx, dy, empty_val, match_mask); 
        }
        else
        {
          rtv = match_image_ncc(im1, K1, R1, t1, im2, K2, R2, t2, depthmap, z_near, z_far, match_cost_tol, match_ratio_threshold, x0, y0, dx, dy, empty_val);
        }
        break;
      case 1:  
        rtv = match_image_grad(im1, K1, R1, t1, im2, K2, R2, t2, depthmap, z_near, z_far, match_cost_tol, match_ratio_threshold, x0, y0, dx, dy, empty_val, match_mask); 
        break;
      default: break;
    }
    return rtv;
  }
  template<typename ImageType, typename DepthType>
  int BinoCorrespondence<typename ImageType, typename DepthType>::match_image_ncc(const ImageType *const *im1,
                                                                                  const DepthType K1[9], const DepthType R1[9], const DepthType t1[3],
                                                                                  const ImageType *const *im2,
                                                                                  const DepthType K2[9], const DepthType R2[9], const DepthType t2[3],
                                                                                  DepthType **depthmap, /*treat im1 as reference view*/
                                                                                  DepthType z_near,
                                                                                  DepthType z_far,
                                                                                  float min_similarity_score_tol,
                                                                                  float match_ratio_threshold, /*threshold for the matching ratio test*/
                                                                                  int x0, /*starting pixel's x coord.*/
                                                                                  int y0, /*starting pixel's y coord.*/
                                                                                  int dx, /*sampling freq. along the x dimension.*/
                                                                                  int dy, /*sampling freq. along the y dimension.*/                                              
                                                                                  DepthType empty_val /*every pixel that has not been matched will be assigned this value*/
                                                                                  )
  {
     int x, y, ix, iy, nr_matched = 0;
     DepthType P1[12], P2[12], F[9];
     DepthType coord2d[3] = { (DepthType)0, (DepthType)0, (DepthType)1 };
     DepthType coord2d_match[3] = { (DepthType)0, (DepthType)0, (DepthType)1 };
     DepthType coord3d[4] = { (DepthType)0, (DepthType)0, (DepthType)0, (DepthType)1 };
     DepthType coord2d_near[3];
     DepthType coord2d_far[3];
     DepthType distance;
     PairviewTriangulationOptimizer<DepthType> Optimizer;

     typedef Pair<float, float>     COORD2D;
     typedef Pair<COORD2D, COORD2D> MATCH2D;
     std::vector<MATCH2D > match_table;
     match_table.reserve( (int)i_div(_width, dx) * (int)i_div(_height, dy) );
     MATCH2D match_pair;

     i_compose_P_matrix_from_K_R_t(K1, R1, t1, P1);
     i_compose_P_matrix_from_K_R_t(K2, R2, t2, P2);
     i_fundamental_from_K_R_t(K1, R1, t1, K2, R2, t2, F);

     const int size = _width * _height;
     i_fill(depthmap[0], size, empty_val);
     for (y = y0; y < _height - y0; y += dy)
     {
       match_pair.first.second = coord2d[1] = (DepthType)y;
       for (x = x0; x < _width - x0; x += dx)
       {
         match_pair.first.first = coord2d[0] = (DepthType)x;
         /*construct the reference patch*/
         CorrespondenceNCC<ImageType> patch_ref;
         if (!patch_ref.init(x, y, _ncc_patch_width, im1, _width, _height))
         {
            continue;
         }
         /*determine search range on the secondary image*/
         i_backproject_through_KRt(coord2d, K1, R1, t1, z_near, coord3d);
         i_project_through_KRt(K2, R2, t2, coord3d, coord2d_near);
         i_backproject_through_KRt(coord2d, K1, R1, t1, z_far,  coord3d);
         i_project_through_KRt(K2, R2, t2, coord3d, coord2d_far);
         if (i_abs(coord2d_near[2]) < Constant<DepthType>::MIN_ABS_SAFE_DIVIDEND() || 
             i_abs(coord2d_far[2])  < Constant<DepthType>::MIN_ABS_SAFE_DIVIDEND())
         {
            continue;
         }
         i_homogeneous_unitize3(coord2d_near);
         i_homogeneous_unitize3(coord2d_far);
         if (match_line_ncc(patch_ref, im2, coord2d_near, coord2d_far, coord2d_match, min_similarity_score_tol, match_ratio_threshold))
         {
           match_pair.second.first  = coord2d_match[0];
           match_pair.second.second = coord2d_match[1];
           match_table.push_back(match_pair); /*record the potential matched pair*/
         }
       }
     }
  
     /*cross check*/
     std::vector<MATCH2D >::const_iterator citer_match = match_table.begin();
     while (citer_match != match_table.end())
     {
       coord2d[0] = citer_match->second.first;
       coord2d[1] = citer_match->second.second;
       coord2d[2] = (DepthType)1.0;
       ix = i_round(coord2d[0]);
       iy = i_round(coord2d[1]);

       CorrespondenceNCC<ImageType> patch_ref;
       if (!patch_ref.init(ix, iy, _ncc_patch_width, im2, _width, _height))
       {
          citer_match++;
          continue;
       }
       
       /*determine search range on the primary image*/
       i_backproject_through_KRt(coord2d, K2, R2, t2, z_near, coord3d);
       i_project_through_KRt(K1, R1, t1, coord3d, coord2d_near);
       i_backproject_through_KRt(coord2d, K2, R2, t2, z_far,  coord3d);
       i_project_through_KRt(K1, R1, t1, coord3d, coord2d_far);
       if (i_abs(coord2d_near[2]) < Constant<DepthType>::MIN_ABS_SAFE_DIVIDEND() ||
           i_abs(coord2d_far[2])  < Constant<DepthType>::MIN_ABS_SAFE_DIVIDEND())
       {
         citer_match++;
         continue;
       }
       
       i_homogeneous_unitize3(coord2d_near);
       i_homogeneous_unitize3(coord2d_far);
       if (match_line_ncc(patch_ref, im1, coord2d_near, coord2d_far, coord2d_match, 0.f, 0.f)/*note, cross check doesn't consider 'min_similarity_score' and 'ratio test'*/)
       {
         distance = i_sqrt(i_sqr(coord2d_match[0] - citer_match->first.first) + i_sqr(coord2d_match[1] - citer_match->first.second));
         if (distance <= _geo_uncert_rad) /*the matched pixel is close enough to the original so the cross check has been passed*/
         {
           coord2d_match[0] = citer_match->first.first;
           coord2d_match[1] = citer_match->first.second;
           coord2d_match[2] = (DepthType)1.0;
           i_triangulate_second_view_error(coord2d_match, coord2d, P1, P2, F, coord3d);
           /*apply non-linear optimization*/
           Optimizer.optimize(coord3d, P1, P2, coord2d_match, coord2d, false);
           Optimizer.getPoint(coord3d);
           if (coord3d[2] <= z_far && coord3d[2] >= z_near)
           {
             ix = i_round(coord2d_match[0]);
             iy = i_round(coord2d_match[1]); /*guaranteed iy and ix are within the image plane*/
             depthmap[iy][ix] = coord3d[2];
             ++nr_matched;
           }
         }
       }
       citer_match++;
     }
     return nr_matched;
  }

  template<typename ImageType, typename DepthType>
  int BinoCorrespondence<typename ImageType, typename DepthType>::match_image_ncc(const ImageType *const *im1,
                                                                                  const DepthType K1[9], const DepthType R1[9], const DepthType t1[3],
                                                                                  const ImageType *const *im2,
                                                                                  const DepthType K2[9], const DepthType R2[9], const DepthType t2[3],
                                                                                  DepthType **depthmap, /*treat im1 as reference view*/
                                                                                  DepthType z_near,
                                                                                  DepthType z_far,
                                                                                  float min_similarity_score_tol,
                                                                                  float match_ratio_threshold, /*threshold for the matching ratio test*/
                                                                                  int x0, /*starting pixel's x coord.*/
                                                                                  int y0, /*starting pixel's y coord.*/
                                                                                  int dx, /*sampling freq. along the x dimension.*/
                                                                                  int dy, /*sampling freq. along the y dimension.*/
                                                                                  DepthType empty_val, /*every pixel that has not been matched will be assigned this value*/
                                                                                  const unsigned char *const *match_mask)

{
  int x, y, ix, iy, nr_matched = 0;
  DepthType P1[12], P2[12], F[9];
  DepthType coord2d[3] = { (DepthType)0, (DepthType)0, (DepthType)1 };
  DepthType coord2d_match[3] = { (DepthType)0, (DepthType)0, (DepthType)1 };
  DepthType coord3d[4] = { (DepthType)0, (DepthType)0, (DepthType)0, (DepthType)1 };
  DepthType coord2d_near[3];
  DepthType coord2d_far[3];
  DepthType distance;
  PairviewTriangulationOptimizer<DepthType> Optimizer;

  typedef Pair<float, float>     COORD2D;
  typedef Pair<COORD2D, COORD2D> MATCH2D;
  std::vector<MATCH2D > match_table;
  match_table.reserve((int)i_div(_width, dx) * (int)i_div(_height, dy));
  MATCH2D match_pair;

  i_compose_P_matrix_from_K_R_t(K1, R1, t1, P1);
  i_compose_P_matrix_from_K_R_t(K2, R2, t2, P2);
  i_fundamental_from_K_R_t(K1, R1, t1, K2, R2, t2, F);

  const int size = _width * _height;
  i_fill(depthmap[0], size, empty_val);
  for (y = y0; y < _height - y0; y += dy)
  {
    match_pair.first.second = coord2d[1] = (DepthType)y;
    for (x = x0; x < _width - x0; x += dx)
    {
      match_pair.first.first = coord2d[0] = (DepthType)x;
      
      if (match_mask[y][x] == 0)
      {
        continue;
      }
        
      /*construct the reference patch*/
      CorrespondenceNCC<ImageType> patch_ref;
      if (!patch_ref.init(x, y, _ncc_patch_width, im1, _width, _height))
      {
        continue;
      }
      /*determine search range on the secondary image*/
      i_backproject_through_KRt(coord2d, K1, R1, t1, z_near, coord3d);
      i_project_through_KRt(K2, R2, t2, coord3d, coord2d_near);
      i_backproject_through_KRt(coord2d, K1, R1, t1, z_far, coord3d);
      i_project_through_KRt(K2, R2, t2, coord3d, coord2d_far);
      if (i_abs(coord2d_near[2]) < Constant<DepthType>::MIN_ABS_SAFE_DIVIDEND() ||
        i_abs(coord2d_far[2])  < Constant<DepthType>::MIN_ABS_SAFE_DIVIDEND())
      {
        continue;
      }
      i_homogeneous_unitize3(coord2d_near);
      i_homogeneous_unitize3(coord2d_far);
      if (match_line_ncc(patch_ref, im2, coord2d_near, coord2d_far, coord2d_match, min_similarity_score_tol, match_ratio_threshold))
      {
        match_pair.second.first = coord2d_match[0];
        match_pair.second.second = coord2d_match[1];
        match_table.push_back(match_pair); /*record the potential matched pair*/
      }
    }
  }

  /*cross check*/
  std::vector<MATCH2D >::const_iterator citer_match = match_table.begin();
  while (citer_match != match_table.end())
  {
    coord2d[0] = citer_match->second.first;
    coord2d[1] = citer_match->second.second;
    coord2d[2] = (DepthType)1.0;
    ix = i_round(coord2d[0]);
    iy = i_round(coord2d[1]);

    CorrespondenceNCC<ImageType> patch_ref;
    if (!patch_ref.init(ix, iy, _ncc_patch_width, im2, _width, _height))
    {
      citer_match++;
      continue;
    }

    /*determine search range on the primary image*/
    i_backproject_through_KRt(coord2d, K2, R2, t2, z_near, coord3d);
    i_project_through_KRt(K1, R1, t1, coord3d, coord2d_near);
    i_backproject_through_KRt(coord2d, K2, R2, t2, z_far, coord3d);
    i_project_through_KRt(K1, R1, t1, coord3d, coord2d_far);
    if (i_abs(coord2d_near[2]) < Constant<DepthType>::MIN_ABS_SAFE_DIVIDEND() ||
      i_abs(coord2d_far[2])  < Constant<DepthType>::MIN_ABS_SAFE_DIVIDEND())
    {
      citer_match++;
      continue;
    }

    i_homogeneous_unitize3(coord2d_near);
    i_homogeneous_unitize3(coord2d_far);
    if (match_line_ncc(patch_ref, im1, coord2d_near, coord2d_far, coord2d_match, 0.f, 0.f)/*note, cross check doesn't consider 'min_similarity_score' and 'ratio test'*/)
    {
      distance = i_sqrt(i_sqr(coord2d_match[0] - citer_match->first.first) + i_sqr(coord2d_match[1] - citer_match->first.second));
      if (distance <= _geo_uncert_rad) /*the matched pixel is close enough to the original so the cross check has been passed*/
      {
        coord2d_match[0] = citer_match->first.first;
        coord2d_match[1] = citer_match->first.second;
        coord2d_match[2] = (DepthType)1.0;
        i_triangulate_second_view_error(coord2d_match, coord2d, P1, P2, F, coord3d);
        /*apply non-linear optimization*/
        Optimizer.optimize(coord3d, P1, P2, coord2d_match, coord2d, false);
        Optimizer.getPoint(coord3d);
        if (coord3d[2] <= z_far && coord3d[2] >= z_near)
        {
          ix = i_round(coord2d_match[0]);
          iy = i_round(coord2d_match[1]); /*guaranteed iy and ix are within the image plane*/
          depthmap[iy][ix] = coord3d[2];
          ++nr_matched;
        }
      }
    }
    citer_match++;
  }
  return nr_matched;
}


  template<typename ImageType, typename DepthType>
  int BinoCorrespondence<typename ImageType, typename DepthType>::match_image_grad(const ImageType *const *im1,
                                                                                       const DepthType K1[9], const DepthType R1[9], const DepthType t1[3],
                                                                                       const ImageType *const *im2,
                                                                                       const DepthType K2[9], const DepthType R2[9], const DepthType t2[3],
                                                                                       DepthType **depthmap, /*treat im1 as reference view*/
                                                                                       DepthType z_near,
                                                                                       DepthType z_far,
                                                                                       float min_similarity_score_tol,
                                                                                       float match_ratio_threshold, /*threshold for the matching ratio test*/
                                                                                       int x0, /*starting pixel's x coord.*/
                                                                                       int y0, /*starting pixel's y coord.*/
                                                                                       int dx, /*sampling freq. along the x dimension.*/
                                                                                       int dy, /*sampling freq. along the y dimension.*/
                                                                                       DepthType empty_val, /*every pixel that has not been matched will be assigned this value*/
                                                                                       const unsigned char *const *match_mask /*binary mask with values 0: non-edge pixel, 1: edge pixel*/)
  {
    int x, y, ix, iy, nr_matched = 0;
    DepthType P1[12], P2[12], F[9];
    DepthType coord2d[3] = { (DepthType)0, (DepthType)0, (DepthType)1 };
    DepthType coord2d_match[3] = { (DepthType)0, (DepthType)0, (DepthType)1 };
    DepthType coord3d[4] = { (DepthType)0, (DepthType)0, (DepthType)0, (DepthType)1 };
    DepthType coord2d_near[3];
    DepthType coord2d_far[3];
    DepthType distance;
    PairviewTriangulationOptimizer<DepthType> Optimizer;

    typedef Pair<float, float> COORD2D;
    typedef Pair<COORD2D, COORD2D> MATCH2D;
    std::vector<MATCH2D > match_table;
    match_table.reserve((int)i_div(_width, dx) * (int)i_div(_height, dy));
    MATCH2D match_pair;

    i_compose_P_matrix_from_K_R_t(K1, R1, t1, P1);
    i_compose_P_matrix_from_K_R_t(K2, R2, t2, P2);
    i_fundamental_from_K_R_t(K1, R1, t1, K2, R2, t2, F);

    const int size = _width * _height;
    i_fill(depthmap[0], size, empty_val);
    for (y = y0; y < _height-y0; y += dy)
    {
      match_pair.first.second = coord2d[1] = (DepthType)y;
      for (x = x0; x < _width-x0; x += dx)
      {
        match_pair.first.first = coord2d[0] = (DepthType)x;

        if (match_mask[y][x] == 0)
        {
          continue;
        }

        /*construct the reference patch*/
        CorrespondenceGrad<ImageType> patch_ref_grad;
        if (!patch_ref_grad.init(x, y, _grad_patch_width, im1, _width, _height))
        {
          continue;
        }
        /*determine search range on the secondary image*/
        i_backproject_through_KRt(coord2d, K1, R1, t1, z_near, coord3d);
        i_project_through_KRt(K2, R2, t2, coord3d, coord2d_near);
        i_backproject_through_KRt(coord2d, K1, R1, t1, z_far, coord3d);
        i_project_through_KRt(K2, R2, t2, coord3d, coord2d_far);
        if (i_abs(coord2d_near[2]) < Constant<DepthType>::MIN_ABS_SAFE_DIVIDEND() ||
            i_abs(coord2d_far[2])  < Constant<DepthType>::MIN_ABS_SAFE_DIVIDEND())
        {
          continue;
        }
        i_homogeneous_unitize3(coord2d_near);
        i_homogeneous_unitize3(coord2d_far);
        if (match_line_grad(patch_ref_grad, im2, coord2d_near, coord2d_far, coord2d_match, min_similarity_score_tol, match_ratio_threshold))
        {
          match_pair.second.first = coord2d_match[0];
          match_pair.second.second = coord2d_match[1];
          match_table.push_back(match_pair); /*record the potential matched pair*/
        }
      }
    }

    /*cross check*/
    std::vector<MATCH2D >::const_iterator citer_match = match_table.begin();
    while (citer_match != match_table.end())
    {
      coord2d[0] = citer_match->second.first;
      coord2d[1] = citer_match->second.second;
      coord2d[2] = (DepthType)1.0;
      ix = i_round(coord2d[0]);
      iy = i_round(coord2d[1]);

      CorrespondenceGrad<ImageType> patch_ref;
      if (!patch_ref.init(ix, iy, _grad_patch_width, im2, _width, _height))
      {
        citer_match++;
        continue;
      }

      /*determine search range on the primary image*/
      i_backproject_through_KRt(coord2d, K2, R2, t2, z_near, coord3d);
      i_project_through_KRt(K1, R1, t1, coord3d, coord2d_near);
      i_backproject_through_KRt(coord2d, K2, R2, t2, z_far, coord3d);
      i_project_through_KRt(K1, R1, t1, coord3d, coord2d_far);
      if (i_abs(coord2d_near[2]) < Constant<DepthType>::MIN_ABS_SAFE_DIVIDEND() ||
          i_abs(coord2d_far[2])  < Constant<DepthType>::MIN_ABS_SAFE_DIVIDEND())
      {
        citer_match++;
        continue;
      }

      i_homogeneous_unitize3(coord2d_near);
      i_homogeneous_unitize3(coord2d_far);
      if (match_line_grad(patch_ref, im1, coord2d_near, coord2d_far, coord2d_match, 0.f, 0.f)/*note, cross check doesn't consider 'min_similarity_score' and 'ratio test'*/)
      {
        distance = i_sqrt(i_sqr(coord2d_match[0] - citer_match->first.first) + i_sqr(coord2d_match[1] - citer_match->first.second));
        if (distance <= _geo_uncert_rad) /*the matched pixel is close enough to the original so the cross check has been passed*/
        {
          coord2d_match[0] = citer_match->first.first;
          coord2d_match[1] = citer_match->first.second;
          coord2d_match[2] = (DepthType)1.0;
          i_triangulate_second_view_error(coord2d_match, coord2d, P1, P2, F, coord3d);
          /*apply non-linear optimization*/
          Optimizer.optimize(coord3d, P1, P2, coord2d_match, coord2d, false);
          Optimizer.getPoint(coord3d);
          if (coord3d[2] <= z_far && coord3d[2] >= z_near)
          {
            ix = i_round(coord2d_match[0]);
            iy = i_round(coord2d_match[1]); /*guaranteed iy and ix are within the image plane*/
            depthmap[iy][ix] = coord3d[2];
            ++nr_matched;
          }
        }
      }
      citer_match++;
    }
    return nr_matched;
  }


  template<typename ImageType, typename DepthType>
  bool BinoCorrespondence<typename ImageType, typename DepthType>::match_line_ncc(const CorrespondenceNCC<ImageType> &patch,
                                                                                  const ImageType *const *im2,
                                                                                  const DepthType line_endpt_coord1[2], /*inhomogeneous 2D image coordinates*/
                                                                                  const DepthType line_endpt_coord2[2], /*inhomogeneous 2D image coordinates*/
                                                                                  DepthType matchpt[2],
                                                                                  float min_similarity_score_tol,
                                                                                  float match_ratio_threshold /*threshold for the matching ratio test*/)
  {
    bool is_matched = false;
    DepthType x, y, start, end, diffx, diffy, slope, offset, matched_x, matched_y, coord_tar[2];
    DepthType coord1_x, coord1_y, coord2_x, coord2_y;
    float score, best, second_best;
    
    coord1_x = line_endpt_coord1[0];
    coord1_y = line_endpt_coord1[1];
    coord2_x = line_endpt_coord2[0];
    coord2_y = line_endpt_coord2[1];
    
    matched_x = matched_y = -(DepthType)1.0;
    best = second_best = -1.0f;
    diffx = coord2_x - coord1_x;
    diffy = coord2_y - coord1_y;
    
    if (i_abs(diffx) == (DepthType)0 && i_abs(diffy) == (DepthType)0)/*a point, not a line*/
    {
      coord_tar[0] = coord1_x;
      coord_tar[1] = coord1_y;
      if (!match_pixel_ncc(patch, im2, coord_tar, best))
      {
        return false;
      }
      else
      {
        matched_x = coord1_x;
        matched_y = coord1_y;
      }
    }
    else if (i_abs(diffx) >= i_abs(diffy)) /*implies that  epipolar_line[1] should not be zero*/
    {
      /*search along the x dim*/
      start = i_max(i_min(coord1_x, coord2_x), (DepthType)0);
      end   = i_min(i_max(coord1_x, coord2_x)+(DepthType)1, (DepthType)_width);
      slope = i_div(diffy, diffx);  /*diffx could not be zero here because |diffx|>|diffy|*/
      offset = coord1_y - slope * coord1_x;
      
      for (x = start; x < end; x += (DepthType)1)
      {
        y = slope*x + offset; //i_div(epipolar_line[0]*x + epipolar_line[2], -epipolar_line[1]);
        coord_tar[0] = x;
        coord_tar[1] = y;
        if( !match_pixel_ncc(patch, im2, coord_tar, score) )
        {
          continue;
        }
        if (score > best)
        {
          second_best = best;
          best = score;
          matched_x = x;
          matched_y = y;
        }
        else 
        {
          if (score > second_best)
          {
            second_best = score;
          }
        }
      }
    }
    else
    {
      /*search along the y dim*/
      start = i_max(i_min(coord1_y, coord2_y), (DepthType)0);
      end = i_min(i_max(coord1_y, coord2_y) + (DepthType)1, (DepthType)_height);
      slope = i_div(diffx, diffy);   //diffy could not be zero here because |diffx|<=|diffy|..
      offset = coord1_x - slope * coord1_y;  //(x*yp - xp*y) / (float)diffy;

      for (y = start; y < end; y += (DepthType)1)
      {
        x = slope*y + offset;
        coord_tar[0] = x;
        coord_tar[1] = y;
        if (!match_pixel_ncc(patch, im2, coord_tar, score))
        {
          continue;
        }
        if (score > best)
        {
          second_best = best;
          best = score;
          matched_x = x;
          matched_y = y;
        }
        else
        {
          if (score > second_best)
          {
            second_best = score;
          }
        }
      }
    }
    if (best >= min_similarity_score_tol && ratiotest(best, second_best, match_ratio_threshold)/*pass ratio test*/)
    { 
      is_matched = true;
    }
    matchpt[0] = matched_x;
    matchpt[1] = matched_y;
    return is_matched;
  }

  template<typename ImageType, typename DepthType>
  bool BinoCorrespondence<typename ImageType, typename DepthType>::match_pixel_ncc(const CorrespondenceNCC<ImageType>& patch,
                                                                                   const ImageType *const *im2,
                                                                                   const DepthType coord[2], /*inhomogeneous 2D image coordinates*/
                                                                                   float &ncc_similarity_score)
  {
    int ix, iy;
    ncc_similarity_score = -1.0f;
    ix = i_round(coord[0]);
    iy = i_round(coord[1]);
    if (i_within_2d(ix, iy, 0, 0, _width, _height))
    {
      CorrespondenceNCC<ImageType> patch_tar;
      if (!patch_tar.init(ix, iy, _ncc_patch_width, im2, _width, _height))
      {
        return false;
      }
      ncc_similarity_score = i_match_ncc(patch, patch_tar);
    }
    else
    {
      return false;
    }
    return true;
  }
  
  template<typename ImageType, typename DepthType>
  bool BinoCorrespondence<typename ImageType, typename DepthType>::match_line_grad(const CorrespondenceGrad<ImageType> &grad_patch,
                                                                                   const ImageType *const *im2,
                                                                                   const DepthType line_endpt_coord1[2], /*inhomogeneous 2D image coordinates*/
                                                                                   const DepthType line_endpt_coord2[2], /*inhomogeneous 2D image coordinates*/
                                                                                   DepthType matchpt[2],
                                                                                   float min_similarity_score_tol,
                                                                                   float match_ratio_threshold /*threshold for the matching ratio test*/)
  {
    bool is_matched = false;
    DepthType x, y, start, end, diffx, diffy, slope, offset, matched_x, matched_y, coord_tar[2];
    DepthType coord1_x, coord1_y, coord2_x, coord2_y;
    float score, best, second_best;

    coord1_x = line_endpt_coord1[0];
    coord1_y = line_endpt_coord1[1];
    coord2_x = line_endpt_coord2[0];
    coord2_y = line_endpt_coord2[1];

    matched_x = matched_y = -(DepthType)1.0;
    best = second_best = -1.0f;
    diffx = coord2_x - coord1_x;
    diffy = coord2_y - coord1_y;

    if (i_abs(diffx) == (DepthType)0 && i_abs(diffy) == (DepthType)0)/*a point, not a line*/
    {
      coord_tar[0] = coord1_x;
      coord_tar[1] = coord1_y;
      if (!match_pixel_grad(grad_patch, im2, coord_tar, best))
      {
        return false;
      }
      else
      {
        matched_x = coord1_x;
        matched_y = coord1_y;
      }
    }
    else if (i_abs(diffx) >= i_abs(diffy)) /*implies that  epipolar_line[1] should not be zero*/
    {
      /*search along the x dim*/
      start = i_max(i_min(coord1_x, coord2_x), (DepthType)0);
      end = i_min(i_max(coord1_x, coord2_x) + (DepthType)1, (DepthType)_width);
      slope = i_div(diffy, diffx);  /*diffx could not be zero here because |diffx|>|diffy|*/
      offset = coord1_y - slope * coord1_x;

      for (x = start; x < end; x += (DepthType)1)
      {
        y = slope*x + offset; //i_div(epipolar_line[0]*x + epipolar_line[2], -epipolar_line[1]);
        coord_tar[0] = x;
        coord_tar[1] = y;
        if (!match_pixel_grad(grad_patch, im2, coord_tar, score))
        {
          continue;
        }
        if (score > best)
        {
          second_best = best;
          best = score;
          matched_x = x;
          matched_y = y;
        }
        else
        {
          if (score > second_best)
          {
            second_best = score;
          }
        }
      }
    }
    else
    {
      /*search along the y dim*/
      start = i_max(i_min(coord1_y, coord2_y), (DepthType)0);
      end = i_min(i_max(coord1_y, coord2_y) + (DepthType)1, (DepthType)_height);
      slope = i_div(diffx, diffy);   //diffy could not be zero here because |diffx|<=|diffy|..
      offset = coord1_x - slope * coord1_y;  //(x*yp - xp*y) / (float)diffy;

      for (y = start; y < end; y += (DepthType)1)
      {
        x = slope*y + offset;
        coord_tar[0] = x;
        coord_tar[1] = y;
        if (!match_pixel_grad(grad_patch, im2, coord_tar, score))
        {
          continue;
        }
        if (score > best)
        {
          second_best = best;
          best = score;
          matched_x = x;
          matched_y = y;
        }
        else
        {
          if (score > second_best)
          {
            second_best = score;
          }
        }
      }
    }
    if (best >= min_similarity_score_tol && ratiotest(best, second_best, match_ratio_threshold)/*pass ratio test*/)
    {
      is_matched = true;
    }
    matchpt[0] = matched_x;
    matchpt[1] = matched_y;
    return is_matched;
  }

  template<typename ImageType, typename DepthType>
  bool BinoCorrespondence<typename ImageType, typename DepthType>::match_pixel_grad(const CorrespondenceGrad<ImageType> &grad_patch,
                                                                                    const ImageType *const *im2,
                                                                                    const DepthType coord[2], /*inhomogeneous 2D image coordinates*/
                                                                                    float &grad_similarity_score)
  {
    int ix, iy;
    grad_similarity_score = 0.0f;
    ix = i_round(coord[0]);
    iy = i_round(coord[1]);
    if (i_within_2d(ix, iy, 0, 0, _width, _height))
    {
      CorrespondenceGrad<ImageType> grad_patch_tar;
      if (!grad_patch_tar.init(ix, iy, _grad_patch_width, im2, _width, _height))
      {
        return false;
      }
      grad_similarity_score = 1.0f - i_match_grad(grad_patch, grad_patch_tar);
    }
    else
    {
      return false;
    }
    return true;
  }
}/* namespace idl */